// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Pose2DTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Initializes a new instance of the Pose2DTest class.
    /// </summary>
    [TestClass]
    public class Pose2DTest
    {
        /// <summary>
        /// Tests the constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DConstructorXY()
        {
            Pose2D pose = new Pose2D(15, -145);
            Assert.AreEqual(pose.X, 15);
            Assert.AreEqual(pose.Y, -145);
            Assert.AreEqual(pose.HasHeading, false);
            Assert.AreEqual(pose.Heading, double.NaN);
        }

        /// <summary>
        /// Tests the constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DConstructorXYT()
        {
            Pose2D pose = new Pose2D(15, -145, 3.12);
            Assert.AreEqual(pose.X, 15);
            Assert.AreEqual(pose.Y, -145);
            Assert.AreEqual(pose.HasHeading, true);
            Assert.AreEqual(pose.Heading, 3.12);
        }

        /// <summary>
        /// Test the ToString operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DToString()
        {
            Pose2D pose = new Pose2D(1, 2, 3);
            string str = pose.ToString();

            Assert.AreEqual(string.Format("<{0}, {1}, {2}rad>", pose.X, pose.Y, pose.Heading), str);
        }

        /// <summary>
        /// Test the equals method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DEqualsMethod()
        {
            Pose2D leftSide = new Pose2D(1, 2, 3);
            Pose2D rightSide = new Pose2D(1, 2, 3);
            Assert.IsTrue(leftSide == rightSide);
            Assert.IsFalse(leftSide == new Pose2D());
        }

        /// <summary>
        /// Test the equals operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DEqualsOperator()
        {
            Pose2D leftSide = new Pose2D(1, 2, 3);
            Pose2D rightSide = new Pose2D(1, 2, 3);
            Assert.IsTrue(leftSide == rightSide);
            Assert.IsFalse(leftSide == new Pose2D());
        }

        /// <summary>
        /// Test the equals operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DNotEqualsOperator()
        {
            Pose2D leftSide = new Pose2D(1, 2, 3);
            Pose2D rightSide = new Pose2D(1, 1, 3);
            Assert.IsTrue(leftSide != rightSide);
            Assert.IsFalse(leftSide != new Pose2D(1, 2, 3));
        }

        /// <summary>
        /// Tests the Hashcode method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DGetHashCode()
        {
            Pose2D pose = new Pose2D(1, 2, 3);
            Assert.AreNotEqual(pose.GetHashCode(), new Pose2D().GetHashCode());
        }

        /// <summary>
        /// Tests the ClampAngle method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DClampAngle()
        {
            Assert.AreEqual((3.2 % MathConstants.TwoPI) - MathConstants.TwoPI, Pose2D.ClampAngle(3.2));
            Assert.AreEqual((-3.2 % MathConstants.TwoPI) + MathConstants.TwoPI, Pose2D.ClampAngle(-3.2));
        }

        /// <summary>
        /// Tests the + operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DPlusOperator()
        {
            Pose2D pose1 = new Pose2D(1, 2, 3);
            Pose2D pose2 = new Pose2D(4, 5, 6);
            Assert.IsTrue(pose1 + pose2 == new Pose2D(5, 7, 9));
        }

        /// <summary>
        /// Tests the - operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DMinusOperator()
        {
            Pose2D pose1 = new Pose2D(1, 7, 3);
            Pose2D pose2 = new Pose2D(4, 5, 7);
            Assert.IsTrue(pose1 - pose2 == new Pose2D(-3, 2, -4));
        }

        /// <summary>
        /// Tests the setters
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DSetters()
        {
            Pose2D pose = new Pose2D(1, 7, 3);
            pose.X = 5;
            pose.Y = 4;
            pose.Heading = 2;
            Assert.IsTrue(pose == new Pose2D(5, 4, 2));
        }

        /// <summary>
        /// Tests the SmallestDeltaAngle
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DSmallestDeltaAngle()
        {
            Pose2D pose = new Pose2D(1, 7, 1);
            Assert.AreEqual(1, pose.SmallestCircularDeltaAngle(2));

            pose = new Pose2D(1, 7, -1);
            Assert.AreEqual(2, pose.SmallestCircularDeltaAngle(1));

            pose = new Pose2D(1, 7, 1);
            Assert.AreEqual(2, pose.SmallestCircularDeltaAngle(-1));
        }

        /// <summary>
        /// Tests the SmallestDeltaAngle for wrapped angles
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DSmallestDeltaAngleWrappedAngles()
        {            
            Assert.AreEqual(0, Pose2D.SmallestCircularDeltaAngle(0, MathConstants.TwoPI * 4), MathConstants.ErrorEpsilon);
            Assert.AreEqual(0, Pose2D.SmallestCircularDeltaAngle(0, 0), MathConstants.ErrorEpsilon);
            Assert.AreEqual(0, Pose2D.SmallestCircularDeltaAngle(MathConstants.PIOverFour, MathConstants.PIOverFour + MathConstants.TwoPI * 4), MathConstants.ErrorEpsilon);            
        }

        /// <summary>
        /// Test the interpolation for normal angles.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DInterpolateNormalAngle()
        {
            long startTime = 10;
            long endTime = 20;
            long[] desiredTimes = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20 };
            double start = MathConstants.PIOverFour;
            double end = MathConstants.PIOverTwo;

            for (int i = 0; i < desiredTimes.Length; ++i)
            {
                double interpolationFactor = 1.0d * (desiredTimes[i] - startTime) / (endTime - startTime);
                double expected = start + Pose2D.SmallestCircularDeltaAngle(end, start) * interpolationFactor;
                double interpolated = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTimes[i], start, end);
                Assert.AreEqual(expected, interpolated, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Test the interpolation for wrapped angles.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DInterpolateWrappedAngle()
        {
            long startTime = 10;
            long endTime = 20;
            long[] desiredTimes = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20 };
            double start = MathConstants.PIOverFour;
            double end = -MathConstants.PIOverTwo;

            for (int i = 0; i < desiredTimes.Length; ++i)
            {
                double interpolationFactor = 1.0d * (desiredTimes[i] - startTime) / (endTime - startTime);
                int sign = Math.Sign(end - start);
                double expected = start + sign * Pose2D.SmallestCircularDeltaAngle(end, start) * interpolationFactor;
                double interpolated = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTimes[i], start, end);
                Assert.AreEqual(expected, interpolated, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Test the interpolation for wrapped angles.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DInterpolateWrappedAngle2()
        {
            long startTime = 10;
            long endTime = 20;
            long[] desiredTimes = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20 };
            double start = 0;
            double end = MathConstants.TwoPI;

            for (int i = 0; i < desiredTimes.Length; ++i)
            {
                double interpolationFactor = 1.0d * (desiredTimes[i] - startTime) / (endTime - startTime);
                int sign = Math.Sign(end - start);
                double expected = start + sign * Pose2D.SmallestCircularDeltaAngle(end, start) * interpolationFactor;
                double interpolated = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTimes[i], start, end);
                Assert.AreEqual(expected, interpolated, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Test the interpolation for wrapped angles.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Pose2DInterpolateWrappedAngle3()
        {
            long startTime = 10;
            long endTime = 20;
            long[] desiredTimes = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20 };
            double start = 0;
            double end = MathConstants.TwoPI * 2;

            for (int i = 0; i < desiredTimes.Length; ++i)
            {
                double interpolationFactor = 1.0d * (desiredTimes[i] - startTime) / (endTime - startTime);
                int sign = Math.Sign(end - start);
                double expected = Pose2D.ClampAngle(start + sign * Pose2D.SmallestCircularDeltaAngle(end, start) * interpolationFactor);
                double interpolated = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTimes[i], start, end);
                Assert.AreEqual(expected, interpolated, MathConstants.ErrorEpsilon);
            }
        }
    }
}
